package FindPath;

/**
 * @Desc:
 * @Author: yanghongjun
 * @Date: 2024/5/23
 **/
import java.util.*;



public class SegmentedAStar {

    // 八个可能的移动方向（上下左右及对角线）
    private static final int[][] dirs = {
            {0, 1}, {1, 0}, {0, -1}, {-1, 0},
            {1, 1}, {1, -1}, {-1, 1}, {-1, -1}
    };

    public static List<List<Node>> segmentedAStar(Node start, Node goal, double segmentDistance) {
        List<List<Node>> segmentedPaths = new ArrayList<>();
        PriorityQueue<Node> openList = new PriorityQueue<>(Comparator.comparingDouble(n -> n.f));
        Set<Node> closedList = new HashSet<>();

        Node currentNode = start;
        while (currentNode != null && !(currentNode.x == goal.x && currentNode.y == goal.y)) {
            openList.add(currentNode);

            while (!openList.isEmpty()) {
                currentNode = openList.poll();

                if (closedList.contains(currentNode)) {
                    continue;
                }

                closedList.add(currentNode);

                // 判断当前路径长度是否达到分段距离
                if (currentNode.g >= segmentDistance) {
                    List<Node> pathSegment = reconstructPath(currentNode);
                    segmentedPaths.add(pathSegment);

                    // 重置起点为当前节点，准备搜索下一段路径
                    currentNode = new Node(currentNode.x, currentNode.y);
                    currentNode.g = 0;
                    currentNode.h = Node.heuristic(currentNode, goal);
                    currentNode.f = currentNode.g + currentNode.h;

                    closedList.clear(); // 清空已关闭列表，以便在新的分段中重新访问节点
                } else {
                    for (int[] dir : dirs) {
                        int newX = currentNode.x + dir[0];
                        int newY = currentNode.y + dir[1];

                        Node neighbor = new Node(newX, newY);
                        if (!closedList.contains(neighbor) && isWalkable(neighbor.x, neighbor.y)) {
                            double tentativeGScore = currentNode.g + 1; // 假设每步代价为1

                            if (!openList.contains(neighbor)) {
                                openList.add(neighbor);
                            } else if (tentativeGScore >= neighbor.g) {
                                continue; // 这不是一条更好的路径
                            }

                            // 这是一条更好的路径，更新节点的信息
                            neighbor.parent = currentNode;
                            neighbor.g = tentativeGScore;
                            neighbor.h = Node.heuristic(neighbor, goal);
                            neighbor.f = neighbor.g + neighbor.h;
                        }
                    }
                }
            }

            if (openList.isEmpty()) {
                break; // 无法找到通往目标的路径
            }
        }

        // 如果最后一个分段没有到达目标，则添加最后一个分段的路径
        if (currentNode != null && currentNode.parent != null) {
            List<Node> lastSegment = reconstructPath(currentNode);
            segmentedPaths.add(lastSegment);
        }

        return segmentedPaths;
    }

    // 从当前节点回溯到起点，重构路径
    private static List<Node> reconstructPath(Node currentNode) {
        List<Node> path = new ArrayList<>();
        while (currentNode != null) {
            path.add(currentNode);
            currentNode = currentNode.parent;
        }
        Collections.reverse(path); // 反转列表以得到正确的路径顺序
        return path;
    }

    // 判断某个位置是否可以行走（示例方法，根据实际情况实现）
    private static boolean isWalkable(int x, int y) {
        // 这里只是一个示例，实际应用中需要实现具体的可行走判断逻辑
        return true;
    }

    public static void main(String[] args) {
        Node start = new Node(0, 0);
        Node goal = new Node(5, 5);
        double segmentDistance = 10; // 设置分段距离阈值，此处为示例值

        List<List<Node>> segmentedPaths = segmentedAStar(start, goal, segmentDistance);
        for (List<Node> pathSegment : segmentedPaths) {
            System.out.println("Path Segment:");
            for (Node node : pathSegment) {
                System.out.println("(" + node.x + ", " + node.y + ")");
            }
            System.out.println();
        }
    }
}

class Node {
    public int x, y;
    public Node parent;
    public double g; // 从起点到当前节点的实际代价
    public double h; // 从当前节点到终点的估计代价（启发式）
    public double f; // f = g + h

    public Node(int x, int y) {
        this.x = x;
        this.y = y;
    }

    // 估计代价，采用曼哈顿距离
    public static double heuristic(Node a, Node b) {
        return Math.abs(a.x - b.x) + Math.abs(a.y - b.y);
    }
}
